/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/

This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:

1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
///btSoftBodyHelpers.cpp by Nathanael Presson

#include "btSoftBodyInternals.h"
#include <stdio.h>
#include <string.h>
#include "btSoftBodyHelpers.h"
#include "btConvexHull.h"

#pragma warning(disable:4996)

//
static void                drawVertex(    btIDebugDraw* idraw,
                                   const btVector3& x,btScalar s,const btVector3& c)
{
    idraw->drawLine(x-btVector3(s,0,0),x+btVector3(s,0,0),c);
    idraw->drawLine(x-btVector3(0,s,0),x+btVector3(0,s,0),c);
    idraw->drawLine(x-btVector3(0,0,s),x+btVector3(0,0,s),c);
}

//
static void                drawBox(    btIDebugDraw* idraw,
                                const btVector3& mins,
                                const btVector3& maxs,
                                const btVector3& color)
{
    const btVector3    c[]={    btVector3(mins.x(),mins.y(),mins.z()),
        btVector3(maxs.x(),mins.y(),mins.z()),
        btVector3(maxs.x(),maxs.y(),mins.z()),
        btVector3(mins.x(),maxs.y(),mins.z()),
        btVector3(mins.x(),mins.y(),maxs.z()),
        btVector3(maxs.x(),mins.y(),maxs.z()),
        btVector3(maxs.x(),maxs.y(),maxs.z()),
        btVector3(mins.x(),maxs.y(),maxs.z())};
    idraw->drawLine(c[0],c[1],color);idraw->drawLine(c[1],c[2],color);
    idraw->drawLine(c[2],c[3],color);idraw->drawLine(c[3],c[0],color);
    idraw->drawLine(c[4],c[5],color);idraw->drawLine(c[5],c[6],color);
    idraw->drawLine(c[6],c[7],color);idraw->drawLine(c[7],c[4],color);
    idraw->drawLine(c[0],c[4],color);idraw->drawLine(c[1],c[5],color);
    idraw->drawLine(c[2],c[6],color);idraw->drawLine(c[3],c[7],color);
}

//
static void                drawTree(    btIDebugDraw* idraw,
                                 const btDbvtNode* node,
                                 int depth,
                                 const btVector3& ncolor,
                                 const btVector3& lcolor,
                                 int mindepth,
                                 int maxdepth)
{
    if(node)
    {
        if(node->isinternal()&&((depth<maxdepth)||(maxdepth<0)))
        {
            drawTree(idraw,node->childs[0],depth+1,ncolor,lcolor,mindepth,maxdepth);
            drawTree(idraw,node->childs[1],depth+1,ncolor,lcolor,mindepth,maxdepth);
        }
        if(depth>=mindepth)
        {
            const btScalar    scl=(btScalar)(node->isinternal()?1:1);
            const btVector3    mi=node->volume.Center()-node->volume.Extents()*scl;
            const btVector3    mx=node->volume.Center()+node->volume.Extents()*scl;
            drawBox(idraw,mi,mx,node->isleaf()?lcolor:ncolor);
        }
    }
}

//
template <typename T>
static inline T                sum(const btAlignedObjectArray<T>& items)
{
    T    v;
    if(items.size())
    {
        v=items[0];
        for(int i=1,ni=items.size();i<ni;++i)
        {
            v+=items[i];
        }
    }
    return(v);
}

//
template <typename T,typename Q>
static inline void            add(btAlignedObjectArray<T>& items,const Q& value)
{
    for(int i=0,ni=items.size();i<ni;++i)
    {
        items[i]+=value;
    }
}

//
template <typename T,typename Q>
static inline void            mul(btAlignedObjectArray<T>& items,const Q& value)
{
    for(int i=0,ni=items.size();i<ni;++i)
    {
        items[i]*=value;
    }
}

//
template <typename T>
static inline T                average(const btAlignedObjectArray<T>& items)
{
    const btScalar    n=(btScalar)(items.size()>0?items.size():1);
    return(sum(items)/n);
}

//
static inline btScalar        tetravolume(const btVector3& x0,
                                        const btVector3& x1,
                                        const btVector3& x2,
                                        const btVector3& x3)
{
    const btVector3    a=x1-x0;
    const btVector3    b=x2-x0;
    const btVector3    c=x3-x0;
    return(dot(a,cross(b,c)));
}

//
#if 0
static btVector3        stresscolor(btScalar stress)
{
    static const btVector3    spectrum[]=    {    btVector3(1,0,1),
        btVector3(0,0,1),
        btVector3(0,1,1),
        btVector3(0,1,0),
        btVector3(1,1,0),
        btVector3(1,0,0),
        btVector3(1,0,0)};
    static const int        ncolors=sizeof(spectrum)/sizeof(spectrum[0])-1;
    static const btScalar    one=1;
    stress=btMax<btScalar>(0,btMin<btScalar>(1,stress))*ncolors;
    const int                sel=(int)stress;
    const btScalar            frc=stress-sel;
    return(spectrum[sel]+(spectrum[sel+1]-spectrum[sel])*frc);
}
#endif

//
void            btSoftBodyHelpers::Draw(    btSoftBody* psb,
                                        btIDebugDraw* idraw,
                                        int drawflags)
{
    const btScalar        scl=(btScalar)0.1;
    const btScalar        nscl=scl*5;
    const btVector3        lcolor=btVector3(0,0,0);
    const btVector3        ncolor=btVector3(1,1,1);
    const btVector3        ccolor=btVector3(1,0,0);
    int i,j,nj;

    /* Nodes    */
    if(0!=(drawflags&fDrawFlags::Nodes))
    {
        for(i=0;i<psb->m_nodes.size();++i)
        {
            const btSoftBody::Node&    n=psb->m_nodes[i];
            if(0==(n.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue;
            idraw->drawLine(n.m_x-btVector3(scl,0,0),n.m_x+btVector3(scl,0,0),btVector3(1,0,0));
            idraw->drawLine(n.m_x-btVector3(0,scl,0),n.m_x+btVector3(0,scl,0),btVector3(0,1,0));
            idraw->drawLine(n.m_x-btVector3(0,0,scl),n.m_x+btVector3(0,0,scl),btVector3(0,0,1));
        }
    }
    /* Links    */
    if(0!=(drawflags&fDrawFlags::Links))
    {
        for(i=0;i<psb->m_links.size();++i)
        {
            const btSoftBody::Link&    l=psb->m_links[i];
            if(0==(l.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue;
            idraw->drawLine(l.m_n[0]->m_x,l.m_n[1]->m_x,lcolor);
        }
    }
    /* Normals    */
    if(0!=(drawflags&fDrawFlags::Normals))
    {
        for(i=0;i<psb->m_nodes.size();++i)
        {
            const btSoftBody::Node&    n=psb->m_nodes[i];
            if(0==(n.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue;
            const btVector3            d=n.m_n*nscl;
            idraw->drawLine(n.m_x,n.m_x+d,ncolor);
            idraw->drawLine(n.m_x,n.m_x-d,ncolor*0.5);
        }
    }
    /* Contacts    */
    if(0!=(drawflags&fDrawFlags::Contacts))
    {
        static const btVector3        axis[]={btVector3(1,0,0),
            btVector3(0,1,0),
            btVector3(0,0,1)};
        for(i=0;i<psb->m_rcontacts.size();++i)
        {
            const btSoftBody::RContact&    c=psb->m_rcontacts[i];
            const btVector3                o=    c.m_node->m_x-c.m_cti.m_normal*
                (dot(c.m_node->m_x,c.m_cti.m_normal)+c.m_cti.m_offset);
            const btVector3                x=cross(c.m_cti.m_normal,axis[c.m_cti.m_normal.minAxis()]).normalized();
            const btVector3                y=cross(x,c.m_cti.m_normal).normalized();
            idraw->drawLine(o-x*nscl,o+x*nscl,ccolor);
            idraw->drawLine(o-y*nscl,o+y*nscl,ccolor);
            idraw->drawLine(o,o+c.m_cti.m_normal*nscl*3,btVector3(1,1,0));
        }
    }
    /* Anchors    */
    if(0!=(drawflags&fDrawFlags::Anchors))
    {
        for(i=0;i<psb->m_anchors.size();++i)
        {
            const btSoftBody::Anchor&    a=psb->m_anchors[i];
            const btVector3                q=a.m_body->getWorldTransform()*a.m_local;
            drawVertex(idraw,a.m_node->m_x,0.25,btVector3(1,0,0));
            drawVertex(idraw,q,0.25,btVector3(0,1,0));
            idraw->drawLine(a.m_node->m_x,q,btVector3(1,1,1));
        }
        for(i=0;i<psb->m_nodes.size();++i)
        {
            const btSoftBody::Node&    n=psb->m_nodes[i];
            if(0==(n.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue;
            if(n.m_im<=0)
            {
                drawVertex(idraw,n.m_x,0.25,btVector3(1,0,0));
            }
        }
    }
    /* Faces    */
    if(0!=(drawflags&fDrawFlags::Faces))
    {
        const btScalar    scl=(btScalar)0.8;
        const btScalar    alp=(btScalar)1;
        const btVector3    col(0,(btScalar)0.7,0);
        for(i=0;i<psb->m_faces.size();++i)
        {
            const btSoftBody::Face&    f=psb->m_faces[i];
            if(0==(f.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue;
            const btVector3            x[]={f.m_n[0]->m_x,f.m_n[1]->m_x,f.m_n[2]->m_x};
            const btVector3            c=(x[0]+x[1]+x[2])/3;
            idraw->drawTriangle((x[0]-c)*scl+c,
                (x[1]-c)*scl+c,
                (x[2]-c)*scl+c,
                col,alp);
        }
    }
    /* Clusters    */
    if(0!=(drawflags&fDrawFlags::Clusters))
    {
        srand(1806);
        for(i=0;i<psb->m_clusters.size();++i)
        {
            if(psb->m_clusters[i]->m_collide)
            {
                btVector3                        color(    rand()/(btScalar)RAND_MAX,
                    rand()/(btScalar)RAND_MAX,
                    rand()/(btScalar)RAND_MAX);
                color=color.normalized()*0.75;
                btAlignedObjectArray<btVector3>    vertices;
                vertices.resize(psb->m_clusters[i]->m_nodes.size());
                for(j=0,nj=vertices.size();j<nj;++j)
                {
                    vertices[j]=psb->m_clusters[i]->m_nodes[j]->m_x;
                }
                HullDesc        hdsc(QF_TRIANGLES,vertices.size(),&vertices[0]);
                HullResult        hres;
                HullLibrary        hlib;
                hdsc.mMaxVertices=vertices.size();
                hlib.CreateConvexHull(hdsc,hres);
                const btVector3    center=average(hres.m_OutputVertices);
                add(hres.m_OutputVertices,-center);
                mul(hres.m_OutputVertices,(btScalar)1);
                add(hres.m_OutputVertices,center);
                for(j=0;j<(int)hres.mNumFaces;++j)
                {
                    const int idx[]={hres.m_Indices[j*3+0],hres.m_Indices[j*3+1],hres.m_Indices[j*3+2]};
                    idraw->drawTriangle(hres.m_OutputVertices[idx[0]],
                        hres.m_OutputVertices[idx[1]],
                        hres.m_OutputVertices[idx[2]],
                        color,1);
                }
                hlib.ReleaseResult(hres);
            }
            /* Velocities    */
#if 0
            for(int j=0;j<psb->m_clusters[i].m_nodes.size();++j)
            {
                const btSoftBody::Cluster&    c=psb->m_clusters[i];
                const btVector3                r=c.m_nodes[j]->m_x-c.m_com;
                const btVector3                v=c.m_lv+cross(c.m_av,r);
                idraw->drawLine(c.m_nodes[j]->m_x,c.m_nodes[j]->m_x+v,btVector3(1,0,0));
            }
#endif
            /* Frame        */
            btSoftBody::Cluster& c=*psb->m_clusters[i];
            idraw->drawLine(c.m_com,c.m_framexform*btVector3(10,0,0),btVector3(1,0,0));
            idraw->drawLine(c.m_com,c.m_framexform*btVector3(0,10,0),btVector3(0,1,0));
            idraw->drawLine(c.m_com,c.m_framexform*btVector3(0,0,10),btVector3(0,0,1));
        }
    }
    /* Notes    */
    if(0!=(drawflags&fDrawFlags::Notes))
    {
        for(i=0;i<psb->m_notes.size();++i)
        {
            const btSoftBody::Note&    n=psb->m_notes[i];
            btVector3                p=n.m_offset;
            for(int j=0;j<n.m_rank;++j)
            {
                p+=n.m_nodes[j]->m_x*n.m_coords[j];
            }
            idraw->draw3dText(p,n.m_text);
        }
    }
    /* Node tree    */
    if(0!=(drawflags&fDrawFlags::NodeTree))        DrawNodeTree(psb,idraw);
    /* Face tree    */
    if(0!=(drawflags&fDrawFlags::FaceTree))        DrawFaceTree(psb,idraw);
    /* Cluster tree    */
    if(0!=(drawflags&fDrawFlags::ClusterTree))    DrawClusterTree(psb,idraw);
    /* Joints        */
    if(0!=(drawflags&fDrawFlags::Joints))
    {
        for(i=0;i<psb->m_joints.size();++i)
        {
            const btSoftBody::Joint*    pj=psb->m_joints[i];
            switch(pj->Type())
            {
            case    btSoftBody::Joint::eType::Linear:
                {
                    const btSoftBody::LJoint*    pjl=(const btSoftBody::LJoint*)pj;
                    const btVector3    a0=pj->m_bodies[0].xform()*pjl->m_refs[0];
                    const btVector3    a1=pj->m_bodies[1].xform()*pjl->m_refs[1];
                    idraw->drawLine(pj->m_bodies[0].xform().getOrigin(),a0,btVector3(1,1,0));
                    idraw->drawLine(pj->m_bodies[1].xform().getOrigin(),a1,btVector3(0,1,1));
                    drawVertex(idraw,a0,0.25,btVector3(1,1,0));
                    drawVertex(idraw,a1,0.25,btVector3(0,1,1));
                }
                break;
            case    btSoftBody::Joint::eType::Angular:
                {
                    const btVector3    o0=pj->m_bodies[0].xform().getOrigin();
                    const btVector3    o1=pj->m_bodies[1].xform().getOrigin();
                    const btVector3    a0=pj->m_bodies[0].xform().getBasis()*pj->m_refs[0];
                    const btVector3    a1=pj->m_bodies[1].xform().getBasis()*pj->m_refs[1];
                    idraw->drawLine(o0,o0+a0*10,btVector3(1,1,0));
                    idraw->drawLine(o0,o0+a1*10,btVector3(1,1,0));
                    idraw->drawLine(o1,o1+a0*10,btVector3(0,1,1));
                    idraw->drawLine(o1,o1+a1*10,btVector3(0,1,1));
                }
            }
        }
    }
}

//
void            btSoftBodyHelpers::DrawInfos(        btSoftBody* psb,
                                             btIDebugDraw* idraw,
                                             bool masses,
                                             bool areas,
                                             bool /*stress*/)
{
    for(int i=0;i<psb->m_nodes.size();++i)
    {
        const btSoftBody::Node&    n=psb->m_nodes[i];
        char                    text[2048]={0};
        char                    buff[1024];
        if(masses)
        {
            sprintf(buff," M(%.2f)",1/n.m_im);
            strcat(text,buff);
        }
        if(areas)
        {
            sprintf(buff," A(%.2f)",n.m_area);
            strcat(text,buff);
        }
        if(text[0]) idraw->draw3dText(n.m_x,text);
    }
}

//
void            btSoftBodyHelpers::DrawNodeTree(    btSoftBody* psb,
                                                btIDebugDraw* idraw,
                                                int mindepth,
                                                int maxdepth)
{
    drawTree(idraw,psb->m_ndbvt.m_root,0,btVector3(1,0,1),btVector3(1,1,1),mindepth,maxdepth);
}

//
void            btSoftBodyHelpers::DrawFaceTree(    btSoftBody* psb,
                                                btIDebugDraw* idraw,
                                                int mindepth,
                                                int maxdepth)
{
    drawTree(idraw,psb->m_fdbvt.m_root,0,btVector3(0,1,0),btVector3(1,0,0),mindepth,maxdepth);
}

//
void            btSoftBodyHelpers::DrawClusterTree(    btSoftBody* psb,
                                                   btIDebugDraw* idraw,
                                                   int mindepth,
                                                   int maxdepth)
{
    drawTree(idraw,psb->m_cdbvt.m_root,0,btVector3(0,1,1),btVector3(1,0,0),mindepth,maxdepth);
}

//
void            btSoftBodyHelpers::DrawFrame(        btSoftBody* psb,
                                             btIDebugDraw* idraw)
{
    if(psb->m_pose.m_bframe)
    {
        static const btScalar    ascl=10;
        static const btScalar    nscl=(btScalar)0.1;
        const btVector3            com=psb->m_pose.m_com;
        const btMatrix3x3        trs=psb->m_pose.m_rot*psb->m_pose.m_scl;
        const btVector3            Xaxis=(trs*btVector3(1,0,0)).normalized();
        const btVector3            Yaxis=(trs*btVector3(0,1,0)).normalized();
        const btVector3            Zaxis=(trs*btVector3(0,0,1)).normalized();
        idraw->drawLine(com,com+Xaxis*ascl,btVector3(1,0,0));
        idraw->drawLine(com,com+Yaxis*ascl,btVector3(0,1,0));
        idraw->drawLine(com,com+Zaxis*ascl,btVector3(0,0,1));
        for(int i=0;i<psb->m_pose.m_pos.size();++i)
        {
            const btVector3    x=com+trs*psb->m_pose.m_pos[i];
            drawVertex(idraw,x,nscl,btVector3(1,0,1));
        }
    }
}

//
btSoftBody*        btSoftBodyHelpers::CreateRope(    btSoftBodyWorldInfo& worldInfo, const btVector3& from,
                                              const btVector3& to,
                                              int res,
                                              int fixeds)
{
    /* Create nodes    */
    const int        r=res+2;
    btVector3*        x=new btVector3[r];
    btScalar*        m=new btScalar[r];
    int i;

    for(i=0;i<r;++i)
    {
        const btScalar    t=i/(btScalar)(r-1);
        x[i]=lerp(from,to,t);
        m[i]=1;
    }
    btSoftBody*        psb= new btSoftBody(&worldInfo,r,x,m);
    if(fixeds&1) psb->setMass(0,0);
    if(fixeds&2) psb->setMass(r-1,0);
    delete[] x;
    delete[] m;
    /* Create links    */
    for(i=1;i<r;++i)
    {
        psb->appendLink(i-1,i);
    }
    /* Finished        */
    return(psb);
}

//
btSoftBody*        btSoftBodyHelpers::CreatePatch(btSoftBodyWorldInfo& worldInfo,const btVector3& corner00,
                                               const btVector3& corner10,
                                               const btVector3& corner01,
                                               const btVector3& corner11,
                                               int resx,
                                               int resy,
                                               int fixeds,
                                               bool gendiags)
{
#define IDX(_x_,_y_)    ((_y_)*rx+(_x_))
    /* Create nodes    */
    if((resx<2)||(resy<2)) return(0);
    const int    rx=resx;
    const int    ry=resy;
    const int    tot=rx*ry;
    btVector3*    x=new btVector3[tot];
    btScalar*    m=new btScalar[tot];
    int iy;

    for(iy=0;iy<ry;++iy)
    {
        const btScalar    ty=iy/(btScalar)(ry-1);
        const btVector3    py0=lerp(corner00,corner01,ty);
        const btVector3    py1=lerp(corner10,corner11,ty);
        for(int ix=0;ix<rx;++ix)
        {
            const btScalar    tx=ix/(btScalar)(rx-1);
            x[IDX(ix,iy)]=lerp(py0,py1,tx);
            m[IDX(ix,iy)]=1;
        }
    }
    btSoftBody*        psb=new btSoftBody(&worldInfo,tot,x,m);
    if(fixeds&1)    psb->setMass(IDX(0,0),0);
    if(fixeds&2)    psb->setMass(IDX(rx-1,0),0);
    if(fixeds&4)    psb->setMass(IDX(0,ry-1),0);
    if(fixeds&8)    psb->setMass(IDX(rx-1,ry-1),0);
    delete[] x;
    delete[] m;
    /* Create links    and faces */
    for(iy=0;iy<ry;++iy)
    {
        for(int ix=0;ix<rx;++ix)
        {
            const int    idx=IDX(ix,iy);
            const bool    mdx=(ix+1)<rx;
            const bool    mdy=(iy+1)<ry;
            if(mdx) psb->appendLink(idx,IDX(ix+1,iy));
            if(mdy) psb->appendLink(idx,IDX(ix,iy+1));
            if(mdx&&mdy)
            {
                if((ix+iy)&1)
                {
                    psb->appendFace(IDX(ix,iy),IDX(ix+1,iy),IDX(ix+1,iy+1));
                    psb->appendFace(IDX(ix,iy),IDX(ix+1,iy+1),IDX(ix,iy+1));
                    if(gendiags)
                    {
                        psb->appendLink(IDX(ix,iy),IDX(ix+1,iy+1));
                    }
                }
                else
                {
                    psb->appendFace(IDX(ix,iy+1),IDX(ix,iy),IDX(ix+1,iy));
                    psb->appendFace(IDX(ix,iy+1),IDX(ix+1,iy),IDX(ix+1,iy+1));
                    if(gendiags)
                    {
                        psb->appendLink(IDX(ix+1,iy),IDX(ix,iy+1));
                    }
                }
            }
        }
    }
    /* Finished        */
#undef IDX
    return(psb);
}

//
btSoftBody*        btSoftBodyHelpers::CreatePatchUV(btSoftBodyWorldInfo& worldInfo,
                                                 const btVector3& corner00,
                                                 const btVector3& corner10,
                                                 const btVector3& corner01,
                                                 const btVector3& corner11,
                                                 int resx,
                                                 int resy,
                                                 int fixeds,
                                                 bool gendiags,
                                                 float* tex_coords)
{

    /*
    *
    *  corners:
    *
    *  [0][0]     corner00 ------- corner01   [resx][0]
    *                |                |
    *                |                |
    *  [0][resy]  corner10 -------- corner11  [resx][resy]
    *
    *
    *
    *
    *
    *
    *   "fixedgs" map:
    *
    *  corner00     -->   +1
    *  corner01     -->   +2
    *  corner10     -->   +4
    *  corner11     -->   +8
    *  upper middle -->  +16
    *  left middle  -->  +32
    *  right middle -->  +64
    *  lower middle --> +128
    *  center       --> +256
    *
    *
    *   tex_coords size   (resx-1)*(resy-1)*12
    *
    *
    *
    *     SINGLE QUAD INTERNALS
    *
    *  1) btSoftBody's nodes and links,
    *     diagonal link is optional ("gendiags")
    *
    *
    *    node00 ------ node01
    *      | .
    *      |   .
    *      |     .
    *      |       .
    *      |         .
    *    node10        node11
    *
    *
    *
    *   2) Faces:
    *      two triangles,
    *      UV Coordinates (hier example for single quad)
    *
    *     (0,1)          (0,1)  (1,1)
    *     1 |\            3 \-----| 2
    *       | \              \    |
    *       |  \              \   |
    *       |   \              \  |
    *       |    \              \ |
    *     2 |-----\ 3            \| 1
    *     (0,0)    (1,0)       (1,0)
    *
    *
    *
    *
    *
    *
    */

#define IDX(_x_,_y_)    ((_y_)*rx+(_x_))
    /* Create nodes        */
    if((resx<2)||(resy<2)) return(0);
    const int    rx=resx;
    const int    ry=resy;
    const int    tot=rx*ry;
    btVector3*    x=new btVector3[tot];
    btScalar*    m=new btScalar[tot];

    int iy;

    for(iy=0;iy<ry;++iy)
    {
        const btScalar    ty=iy/(btScalar)(ry-1);
        const btVector3    py0=lerp(corner00,corner01,ty);
        const btVector3    py1=lerp(corner10,corner11,ty);
        for(int ix=0;ix<rx;++ix)
        {
            const btScalar    tx=ix/(btScalar)(rx-1);
            x[IDX(ix,iy)]=lerp(py0,py1,tx);
            m[IDX(ix,iy)]=1;
        }
    }
    btSoftBody*    psb=new btSoftBody(&worldInfo,tot,x,m);
    if(fixeds&1)        psb->setMass(IDX(0,0),0);
    if(fixeds&2)        psb->setMass(IDX(rx-1,0),0);
    if(fixeds&4)        psb->setMass(IDX(0,ry-1),0);
    if(fixeds&8)        psb->setMass(IDX(rx-1,ry-1),0);
    if(fixeds&16)        psb->setMass(IDX((rx-1)/2,0),0);
    if(fixeds&32)        psb->setMass(IDX(0,(ry-1)/2),0);
    if(fixeds&64)        psb->setMass(IDX(rx-1,(ry-1)/2),0);
    if(fixeds&128)        psb->setMass(IDX((rx-1)/2,ry-1),0);
    if(fixeds&256)        psb->setMass(IDX((rx-1)/2,(ry-1)/2),0);
    delete[] x;
    delete[] m;


    int z = 0;
    /* Create links    and faces    */
    for(iy=0;iy<ry;++iy)
    {
        for(int ix=0;ix<rx;++ix)
        {
            const bool    mdx=(ix+1)<rx;
            const bool    mdy=(iy+1)<ry;

            int node00=IDX(ix,iy);
            int node01=IDX(ix+1,iy);
            int node10=IDX(ix,iy+1);
            int node11=IDX(ix+1,iy+1);

            if(mdx) psb->appendLink(node00,node01);
            if(mdy) psb->appendLink(node00,node10);
            if(mdx&&mdy)
            {
                psb->appendFace(node00,node10,node11);
                if (tex_coords) {
                    tex_coords[z+0]=CalculateUV(resx,resy,ix,iy,0);
                    tex_coords[z+1]=CalculateUV(resx,resy,ix,iy,1);
                    tex_coords[z+2]=CalculateUV(resx,resy,ix,iy,0);
                    tex_coords[z+3]=CalculateUV(resx,resy,ix,iy,2);
                    tex_coords[z+4]=CalculateUV(resx,resy,ix,iy,3);
                    tex_coords[z+5]=CalculateUV(resx,resy,ix,iy,2);
                }
                psb->appendFace(node11,node01,node00);
                if (tex_coords) {
                    tex_coords[z+6 ]=CalculateUV(resx,resy,ix,iy,3);
                    tex_coords[z+7 ]=CalculateUV(resx,resy,ix,iy,2);
                    tex_coords[z+8 ]=CalculateUV(resx,resy,ix,iy,3);
                    tex_coords[z+9 ]=CalculateUV(resx,resy,ix,iy,1);
                    tex_coords[z+10]=CalculateUV(resx,resy,ix,iy,0);
                    tex_coords[z+11]=CalculateUV(resx,resy,ix,iy,1);
                }
                if (gendiags) psb->appendLink(node00,node11);
                z += 12;
            }
        }
    }
    /* Finished    */
#undef IDX
    return(psb);
}

float   btSoftBodyHelpers::CalculateUV(int resx,int resy,int ix,int iy,int id)
{

    /*
    *
    *
    *    node00 --- node01
    *      |          |
    *    node10 --- node11
    *
    *
    *   ID map:
    *
    *   node00 s --> 0
    *   node00 t --> 1
    *
    *   node01 s --> 3
    *   node01 t --> 1
    *
    *   node10 s --> 0
    *   node10 t --> 2
    *
    *   node11 s --> 3
    *   node11 t --> 2
    *
    *
    */

    float tc=0.0f;
    if (id == 0) {
        tc = (1.0f/((resx-1))*ix);
    }
    else if (id==1) {
        tc = (1.0f/((resy-1))*(resy-1-iy));
    }
    else if (id==2) {
        tc = (1.0f/((resy-1))*(resy-1-iy-1));
    }
    else if (id==3) {
        tc = (1.0f/((resx-1))*(ix+1));
    }
    return tc;
}
//
btSoftBody*        btSoftBodyHelpers::CreateEllipsoid(btSoftBodyWorldInfo& worldInfo,const btVector3& center,
                                                   const btVector3& radius,
                                                   int res)
{
    struct    Hammersley
    {
        static void    Generate(btVector3* x,int n)
        {
            for(int i=0;i<n;i++)
            {
                btScalar    p=0.5,t=0;
                for(int j=i;j;p*=0.5,j>>=1) if(j&1) t+=p;
                btScalar    w=2*t-1;
                btScalar    a=(SIMD_PI+2*i*SIMD_PI)/n;
                btScalar    s=btSqrt(1-w*w);
                *x++=btVector3(s*btCos(a),s*btSin(a),w);
            }
        }
    };
    btAlignedObjectArray<btVector3>    vtx;
    vtx.resize(3+res);
    Hammersley::Generate(&vtx[0],vtx.size());
    for(int i=0;i<vtx.size();++i)
    {
        vtx[i]=vtx[i]*radius+center;
    }
    return(CreateFromConvexHull(worldInfo,&vtx[0],vtx.size()));
}



//
btSoftBody*        btSoftBodyHelpers::CreateFromTriMesh(btSoftBodyWorldInfo& worldInfo,const btScalar*    vertices,
                                                     const int* triangles,
                                                     int ntriangles)
{
    int        maxidx=0;
    int i,j,ni;

    for(i=0,ni=ntriangles*3;i<ni;++i)
    {
        maxidx=btMax(triangles[i],maxidx);
    }
    ++maxidx;
    btAlignedObjectArray<bool>        chks;
    btAlignedObjectArray<btVector3>    vtx;
    chks.resize(maxidx*maxidx,false);
    vtx.resize(maxidx);
    for(i=0,j=0,ni=maxidx*3;i<ni;++j,i+=3)
    {
        vtx[j]=btVector3(vertices[i],vertices[i+1],vertices[i+2]);
    }
    btSoftBody*        psb=new btSoftBody(&worldInfo,vtx.size(),&vtx[0],0);
    for( i=0,ni=ntriangles*3;i<ni;i+=3)
    {
        const int idx[]={triangles[i],triangles[i+1],triangles[i+2]};
#define IDX(_x_,_y_) ((_y_)*maxidx+(_x_))
        for(int j=2,k=0;k<3;j=k++)
        {
            if(!chks[IDX(idx[j],idx[k])])
            {
                chks[IDX(idx[j],idx[k])]=true;
                chks[IDX(idx[k],idx[j])]=true;
                psb->appendLink(idx[j],idx[k]);
            }
        }
#undef IDX
        psb->appendFace(idx[0],idx[1],idx[2]);
    }
    psb->randomizeConstraints();
    return(psb);
}

//
btSoftBody*        btSoftBodyHelpers::CreateFromConvexHull(btSoftBodyWorldInfo& worldInfo,    const btVector3* vertices,
                                                        int nvertices)
{
    HullDesc        hdsc(QF_TRIANGLES,nvertices,vertices);
    HullResult        hres;
    HullLibrary        hlib;/*??*/
    hdsc.mMaxVertices=nvertices;
    hlib.CreateConvexHull(hdsc,hres);
    btSoftBody*        psb=new btSoftBody(&worldInfo,(int)hres.mNumOutputVertices,
        &hres.m_OutputVertices[0],0);
    for(int i=0;i<(int)hres.mNumFaces;++i)
    {
        const int idx[]={    hres.m_Indices[i*3+0],
            hres.m_Indices[i*3+1],
            hres.m_Indices[i*3+2]};
        if(idx[0]<idx[1]) psb->appendLink(    idx[0],idx[1]);
        if(idx[1]<idx[2]) psb->appendLink(    idx[1],idx[2]);
        if(idx[2]<idx[0]) psb->appendLink(    idx[2],idx[0]);
        psb->appendFace(idx[0],idx[1],idx[2]);
    }
    hlib.ReleaseResult(hres);
    psb->randomizeConstraints();
    return(psb);
}
